#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 将gazebo的定位数据转为pose 2021年1月8日
import os
import time
import numpy as np
import math
import rospy
from std_msgs.msg import Float32
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf
from std_msgs.msg import Int32
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import Point
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Quaternion
import time

f = 50.0

class gazebo_to_pos:
    def __init__(self):
        # 设置循环的频率
        rate = rospy.Rate(f)
        rospy.Subscriber('/gazebo/model_states', ModelStates, self.get_pose)
        # Main while loop.
        while not rospy.is_shutdown():
            rate.sleep()

    def get_pose(self, states):
        for i in range(len(states.name)):
            if states.name[i] == 'car1':
                car1_position = states.pose[i].position
                car1_quaternion = states.pose[i].orientation
                pos1_pub.publish(car1_position)
                qua1_pub.publish(car1_quaternion)
            elif states.name[i] == 'car2':
                car2_position = states.pose[i].position
                car2_quaternion = states.pose[i].orientation
                pos2_pub.publish(car2_position)
                qua2_pub.publish(car2_quaternion)
            elif states.name[i] == 'car3':
                car3_position = states.pose[i].position
                car3_quaternion = states.pose[i].orientation
                pos3_pub.publish(car3_position)
                qua3_pub.publish(car3_quaternion)
            elif states.name[i] == 'car4':
                car4_position = states.pose[i].position
                car4_quaternion = states.pose[i].orientation
                pos4_pub.publish(car4_position)
                qua4_pub.publish(car4_quaternion)


if __name__ == '__main__':
    # ROS节点初始化
    rospy.init_node('gazebo_to_pos', anonymous=True)
    try:
        pos1_pub = rospy.Publisher('/car1/position', Point, queue_size=10)
        qua1_pub = rospy.Publisher('/car1/quaternion', Quaternion, queue_size=10)
        pos2_pub = rospy.Publisher('/car2/position', Point, queue_size=10)
        qua2_pub = rospy.Publisher('/car2/quaternion', Quaternion, queue_size=10)
        pos3_pub = rospy.Publisher('/car3/position', Point, queue_size=10)
        qua3_pub = rospy.Publisher('/car3/quaternion', Quaternion, queue_size=10)
        pos4_pub = rospy.Publisher('/car4/position', Point, queue_size=10)
        qua4_pub = rospy.Publisher('/car4/quaternion', Quaternion, queue_size=10)
        gazebo_to_pos()
    except rospy.ROSInterruptException:
        pass
